/* Program to track and hunt down a blob. Make sure to calibrate the color tracking, and
to have good lighting! Robot will turn in a circle until it sees a sufficiently large blob. 
It will then center on the blob and move at it, adjusting its course along the way.


If the touch sensor is pressed, the robot will stop and attempt to grab onto whatever
triggered the switch, lifting it off the ground.*/




#use "xbccamlib.ic"


#define CLAW_HAND 3              //Servo for the claw hand. Range is from 0 (closed) to 255 (open).
#define CLAW_ARM 2               //Servo for the claw arm. Range is from 0 (down) to 140 (up).
#define TOUCH_SENSOR digital(15) //Read the touch sensor on pin#


#define BLOB_MIN_SIZE 500        //Minimum blob size in square pixels
#define CENTER 176               //Center x pixel of the camera
#define ERROR_RANGE 70           //Range to the left and right of the center pixel
#define SPEED 100                //Motor speed
#define TURN_SPEED 60            //Motor speed when making a turn
#define MR 1                     //Right Motor pin#
#define ML 0                     //Left Motor pin#
int last_seen_right = 1;         //Holds last known direction of blob






int closeclaw(){                               
    set_servo_position(CLAW_HAND,160);
}
int openclaw(){
    set_servo_position(CLAW_HAND,255);
}
int lowerclaw(){
    set_servo_position(CLAW_ARM,40);
}
int raiseclaw(){
    set_servo_position(CLAW_ARM,128);
}






int main(){
    init_camera();
    enable_servos();
    openclaw();                                                      //sets claw open to begin
    lowerclaw();
    
    while(!b_button()){
        
        if(TOUCH_SENSOR){                                            //if touch sensor is pressed,
            ao();                                                    //turn off motors and close and raise claw
            sleep(1.0);
            closeclaw();
            sleep(1.0);
            raiseclaw();
        }
        
        else {                                                       //if the touch sensor isn't pressed, look for a blob
            track_update();
            
            if(track_size(0,0) > BLOB_MIN_SIZE){                     //if there is a big enough blob
                
                freeze(MR);
                freeze(ML);
                
                if(last_seen_right){
                    motor(MR,TURN_SPEED);
                    motor(ML,0);
                    sleep(.1);
                    ao();
                }
                else if(!last_seen_right){
                    motor(MR,0);
                    motor(ML,TURN_SPEED);
                    sleep(.1);
                    ao();
                }
                
                
                if(track_x(0,0) < (CENTER - ERROR_RANGE +20)){           //if blob is on the left,
                    last_seen_right=0;                               //turn left 
                    motor(MR,TURN_SPEED);
                    motor(ML,0);
                    //sleep(.1);
                    //ao();
                }
                else if(track_x(0,0) > (CENTER + ERROR_RANGE +20)){      //if blob is on the right,
                    last_seen_right=1;                               //turn right 
                    motor(MR,0);
                    motor(ML,TURN_SPEED);
                    //sleep(.1);
                    //ao();
                }
                else{                                               //if blob is within ERROR_RANGE,
                    //ao();                                         //go forward 
                    //sleep(.1);
                    
                    motor(MR,SPEED);
                    motor(ML,SPEED);
                    sleep(.2);
                }
                
            } //end if
            
            else{                                                   //if no blob found,
                if(last_seen_right){                                //turn in last known direction
                    motor(MR,-TURN_SPEED);
                    motor(ML,TURN_SPEED);
                }
                else{
                    motor(MR,TURN_SPEED);
                    motor(ML,-TURN_SPEED);
                }
            } //end else
            
        }        
    } //end while
    
    ao();
    openclaw();                                                     //open claw and
    lowerclaw(); 
    sleep(1.0);
    disable_servos();                                               //turn off when B button pressed
}
